#ifndef _GRADIENT_H_
#define _GRADIENT_H_

#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <sstream> 
#include <stdio.h>

#include "Integral.h"
#include "voronoi.h"
#include "smart_print.h"

#define K1 (K * 2)

using namespace std;

typedef vector<double> DataList;

void InitData();
void GradSearch(DataList &p, double sigma,double epsilon);
void QMin(DataList &de_dxi, DataList &p, double epsilon, double sigma);
void getDfDx(DataList &de_dxi, DataList &p);
string CreateResultString(DataList &pMin, double yMin);
double getFunc(DataList &p);
string stringify(double x);

DataList p1;
DataList p2;
DataList pMin;
DataList de_dxi;

double yMin;
double err;
double z0;
double h;

int j;
vector<polygon> polygons;

void GradSearch(DataList &p, double sigma, double epsilon)
{
    int max = 600;
    h = 1;
    err = 1;
    int count = 0;

    while(count < max && (h>sigma ||err > epsilon))
    {
        build_voronoi(p, polygons);
        getDfDx(de_dxi, p);
        QMin(de_dxi, p, epsilon, sigma);

        for(int i = 0; i<K1; i++)
            p.at(i) = pMin.at(i);

        z0 = yMin;
        count = count + j + 1;
    }
    cout<<CreateResultString(pMin, yMin)<<endl;
}

void QMin(DataList &de_dxi, DataList &p, double epsilon, double sigma)
{
    int cond = 0;
    int jmax = 60;

    z0 = getFunc(p);

    for(int i = 0; i<K1; i++)
        p1.at(i) = p.at(i) + h * de_dxi.at(i);

    double y1 = getFunc(p1);

    for(int i = 0; i<K1; i++)
        p2.at(i) = p.at(i) + 2 * h * de_dxi.at(i);

    double y2 = getFunc(p2);

    j = 0;

    while (j<jmax && cond == 0)
    {
        if (z0<=y1)
        {
            for(int i = 0; i<K1; i++)
                p2.at(i) = p1.at(i);

            y2 = y1;
            h = h / 2;

            for(int i = 0; i<K1; i++)
                p1.at(i) = p.at(i) + h * de_dxi.at(i);

            y1 = getFunc(p1);
        }
        else if (y2 < y1)
        {
            for(int i = 0; i<K1; i++)
                p1.at(i) = p2.at(i);

            y1 = y2;

            h = h*2;

            for(int i = 0; i<K1; i++)
                p2.at(i) = p.at(i) + 2 * h * de_dxi.at(i);

            y2 = getFunc(p2);
        }
        else
        {
            cond = -1;
        }

        j = j+1;
        if (h < sigma)
            cond = 1;
    }

    double hMin = (h/2)* (4 * y1 - 3* z0 - y2) / (2* y1 - z0 - y2);

    for (int i = 0; i< K1; i++)
    {
        pMin.at(i) = p.at(i) + hMin * de_dxi.at(i);
    }

    yMin = getFunc(pMin);

    double h0 = fabs(hMin);
    double h1 = fabs(hMin - h);
    double h2 = fabs(hMin - 2* h);

    if (h0 < h)
        h = h0;
    if (h1 < h)
        h = h1;
    if (h2 < h)
        h = h2;
    if (h == 0)
        h = hMin;
    if (h < sigma)
        cond = 1;
    double e0 = fabs(z0 - yMin);
    double e1 = fabs(y1 - yMin);
    double e2 = fabs(y2 - yMin);

    if (e0 != 0 && e0 < err)
        err = e0;
    if (e1 != 0 && e1 < err)
        err = e1;
    if (e2 != 0 && e2 < err)
        err = e2;
    if (e0 == 0 && e1 == 0 && e2 == 0)
        err = 0;
    if (err < epsilon)
        cond = 2;
}

double getFunc(DataList &p) {
    return integral2(p.data(), peopleDensity) - integral2(p.data(), industryDensity);
}

bool isInPolygon(double x, double y, const polygon& p) {
    int r = 0, t;
    for (int i = 0; i < p.size(); ++i) {
        Vertex v0 = vertices[p[i]], v1 = vertices[p[(i + 1) % p.size()]];
        t = (y - v0.y) * (v1.x - v0.x) < (x - v0.x) * (v1.y - v0.y) ? -1 : 1;
        if (t != r) {
            if (r == 0) {
                r = t;
            } else {
                return false;
            }
        }
    }
    return true;
}

#define EPS 0.000001
#define EQ(x, y) ((x) - (y) < EPS)

void getDfDx(DataList & de_dxi, DataList &p) {
    for (int k = 0; k < de_dxi.size(); ++k) {
        de_dxi[k] = 0;
    }
    int s = 0, ss = 0;
    for (int i = 0; i < N; ++i) {
        double x = double(i) / N;
        for (int j = 0; j < N; ++j) {
            double y = double(j) / N;
            if ((0.5 - x) * (0.5 - x) + (0.5 - y) * (0.5 - y) > 0.25) {
                if (y > 0.5) {
                    break;
                } else {
                    continue;
                }
            }
            for (int k = 0; k < polygons.size(); ++k) {
                ++ss;
                if (isInPolygon(x, y, polygons[k])) {
                    if (!EQ(p[2 * k], x)) {
                        de_dxi[2 * k] += (p[2 * k] - x) / sqrt((p[2 * k] - x) * (p[2 * k] - x) + (p[2 * k + 1] - y) * (p[2 * k + 1] - y)) * (peopleDensity[i * N + j] - industryDensity[i * N + j]);
                    }
                    if (!EQ(p[2 * k + 1], y)) {
                        de_dxi[2 * k + 1] += (p[2 * k + 1] - y) / sqrt((p[2 * k] - x) * (p[2 * k] - x) + (p[2 * k + 1] - y) * (p[2 * k + 1] - y)) * (peopleDensity[i * N + j] - industryDensity[i * N + j]);
                    }
                    ++s;
                    break;
                }
            }
        }
    }
    for (int k = 0; k < de_dxi.size(); ++k) {
        de_dxi[k] /= s;
    }
}

void InitData()
{
    for(int i = 0; i<K1; i++)
    {
        p1.push_back(0);
        p2.push_back(0);
        pMin.push_back(0);
        de_dxi.push_back(0);
    }
}

string stringify(double x)
{
    ostringstream o;
    if (!(o << x))
        return 0;
    return o.str();
}

string CreateResultString(DataList &pMin,double yMin)
{
    string resultStr = "f[";

    for(int i = 0; i<K1; i++)
    {
        if (i != 0)
            resultStr += ",";
        resultStr += stringify(pMin.at(i));
    }

    resultStr += "] = " + stringify(yMin);

    return resultStr;
}

#endif
